/*
 * ser_proto.c
 *
 *  Created on: 09.06.2010
 *      Author: Phil
 */

#include "ser_proto.h"

#define PROTO_BYTE_STX     ((unsigned char)0xfe)
#define PROTO_BYTE_ESC     ((unsigned char)0xfd)
#define PROTO_BYTE_ETX     ((unsigned char)0xfc)

#define RX_STATE_IDLE       0
#define RX_STATE_STX        1
#define RX_STATE_DATA1      2
#define RX_STATE_DATA       3
#define RX_STATE_ESC1       4
#define RX_STATE_ESC        5

#define CHECK_ESCAPE(b) ((b) == PROTO_BYTE_STX \
		|| (b) == PROTO_BYTE_ESC \
		|| (b) == PROTO_BYTE_ETX)


#define ENCODE_CHECK_AND_APPEND(byte)			\
	if(dest_pos < dest_size)					\
	{											\
		dest[dest_pos++] = (byte);				\
	}											\
	else										\
	{											\
		return -1;								\
	}

int SerProto_Encode(unsigned char *dest, unsigned int dest_size,
		const unsigned char *packet, unsigned int packet_size)
{
	unsigned int dest_pos = 0;
	unsigned char checksum = 0;
	unsigned int packet_pos;

	ENCODE_CHECK_AND_APPEND(PROTO_BYTE_STX);
	for(packet_pos = 0; packet_pos < packet_size; ++packet_pos)
	{
		unsigned char byte = packet[packet_pos];
		checksum += byte;
		if(CHECK_ESCAPE(byte))
		{
			ENCODE_CHECK_AND_APPEND(PROTO_BYTE_ESC);
			byte &= ~0x80;
		}
		ENCODE_CHECK_AND_APPEND(byte);
	}

	if(CHECK_ESCAPE(checksum)) {
		ENCODE_CHECK_AND_APPEND(PROTO_BYTE_ESC);
		checksum &= ~0x80;
	}
	ENCODE_CHECK_AND_APPEND(checksum);
	ENCODE_CHECK_AND_APPEND(PROTO_BYTE_ETX);
	return dest_pos;
}

void SerProto_DecodeInit(SerProtoDecodeState *state, unsigned char *dest,
		unsigned int dest_size)
{
	state->state = RX_STATE_STX;
	state->dest = dest;
	state->dest_size = dest_size;
}

int SerProto_DecodeLength(SerProtoDecodeState *state)
{
	return state->state == RX_STATE_IDLE ? state->dest_pos : -1;
}

unsigned int SerProto_Decode(SerProtoDecodeState *state, const unsigned char *data,
		unsigned int data_pos, unsigned int data_size)
{
	unsigned char rx_state = state->state;
	for(; data_pos < data_size && rx_state != RX_STATE_IDLE; ++data_pos)
	{
		unsigned char rxByte = data[data_pos];
		if(rxByte == PROTO_BYTE_STX) {
			// reset state
			state->dest_pos = 0;
			state->checksum = 0;
			state->byte = 0;
			rx_state = RX_STATE_DATA1;
		} else switch(rx_state) {
			case RX_STATE_STX:
				// nothing to do
				break;

			case RX_STATE_DATA1:
				if(rxByte == PROTO_BYTE_ETX) {
					rx_state = RX_STATE_STX;
				} else if (rxByte == PROTO_BYTE_ESC) {
					rx_state = RX_STATE_ESC1;
				} else {
					state->byte = rxByte;
					rx_state = RX_STATE_DATA;
				}
				break;

			case RX_STATE_ESC1:
				if(rxByte == PROTO_BYTE_ETX) {
					rx_state = RX_STATE_STX;
				} else if (rxByte == PROTO_BYTE_ESC) {
					rx_state = RX_STATE_STX;
				} else {
					state->byte = rxByte | ~0x7f;
					rx_state = RX_STATE_DATA;
				}
				break;

			case RX_STATE_DATA:
				if(rxByte == PROTO_BYTE_ETX) {
					if(state->byte == state->checksum) {
						rx_state = RX_STATE_IDLE;
					} else {
						rx_state = RX_STATE_STX;
					}
				} else if (rxByte == PROTO_BYTE_ESC) {
					rx_state = RX_STATE_ESC;
				} else {
					state->checksum += state->byte;
					if(state->dest_pos < state->dest_size) {
						state->dest[state->dest_pos++] = state->byte;
					}
					state->byte = rxByte;
					rx_state = RX_STATE_DATA;
				}
				break;

			case RX_STATE_ESC:
				if(rxByte == PROTO_BYTE_ETX) {
					rx_state = RX_STATE_STX;
				} else if (rxByte == PROTO_BYTE_ESC) {
					rx_state = RX_STATE_STX;
				} else {
					state->checksum += state->byte;
					if(state->dest_pos < state->dest_size) {
						state->dest[state->dest_pos++] = state->byte;
					}
					state->byte = rxByte | 0x80;
					rx_state = RX_STATE_DATA;
				}
				break;

			default:
			case RX_STATE_IDLE:
				rx_state = RX_STATE_IDLE;
				break;
		}
	}
	state->state = rx_state;
	return data_pos;
}
